/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "estimator.h"




#if 0
void omega2_filter(void)
{

    float height, dd_height, _climb_rate, _hgtCompFiltOmega;
    /*
          use a complimentary filter to calculate climb_rate. This is
          designed to minimise lag
         */
    float baro_alt = _ahrs.get_baro().get_altitude();
    // Get height acceleration
    float hgt_ddot_mea = -(_ahrs.get_accel_ef().z + GRAVITY_MSS);
    // Perform filter calculation using backwards Euler integration
    // Coefficients selected to place all three filter poles at omega
    float omega2 = _hgtCompFiltOmega * _hgtCompFiltOmega;
    float hgt_err = baro_alt - height;
    float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega;

    dd_height += integ1_input * DT;

    float integ2_input = dd_height + hgt_ddot_mea + hgt_err * omega2 * 3.0f;

    _climb_rate += integ2_input * DT;

    float integ3_input = _climb_rate + hgt_err * _hgtCompFiltOmega * 3.0f;
    // If more than 1 second has elapsed since last update then reset the integrator state
    // to the measured height
    if (DT > 1.0f)
    {
        height = _height;
    }
    else
    {
        height += integ3_input * DT;
    }
}

#define TIME_CONTANST_Z5 .0f
#define K_ACC_Z (5.0f / (TIME_CONTANST_Z * TIME_CONTANST_Z * TIME_CONTANST_Z))
#define K_VEL_Z (3.0f / (TIME_CONTANST_Z * TIME_CONTANST_Z))
#define K_POS_Z (3.0f / TIME_CONTANST_Z)

float High_Filter[3] = {
    0.03, // 0.015
    0.05, // 0.05
    0.02  // 0.03
};

float Altitude_Dealt = 0;
void Strapdown_INS_High()
{
    // Altitude_Dealt=HC_SR04_Distance-NamelessQuad.Position[_YAW];//气压计(超声波)与SINS估计量的差，单位cm
    Altitude_Dealt = Altitude_Estimate - NamelessQuad.Position[_YAW]; // 气压计(超声波)与SINS估计量的差，单位cm

    acc_correction[_YAW] += High_Filter[0] * Altitude_Dealt * K_ACC_Z; // 加速度校正量
    vel_correction[_YAW] += High_Filter[1] * Altitude_Dealt * K_VEL_Z; // 速度校正量
    pos_correction[_YAW] += High_Filter[2] * Altitude_Dealt * K_POS_Z; // 位置校正量

    // 原始加速度+加速度校正量=融合后的加速度
    NamelessQuad.Acceleration[_YAW] = Origion_NamelessQuad.Acceleration[_YAW] + acc_correction[_YAW];

    // 融合后的加速度积分得到速度增量
    SpeedDealt[_YAW] = NamelessQuad.Acceleration[_YAW] * CNTLCYCLE;

    // 得到速度增量后，更新原始位置
    Origion_NamelessQuad.Position[_YAW] += (NamelessQuad.Speed[_YAW] + 0.5 * SpeedDealt[_YAW]) * CNTLCYCLE;

    // 原始位置+位置校正量=融合后位置
    NamelessQuad.Position[_YAW] = Origion_NamelessQuad.Position[_YAW] + pos_correction[_YAW];

    // 原始速度+速度校正量=融合后的速度
    Origion_NamelessQuad.Speed[_YAW] += SpeedDealt[_YAW];
    NamelessQuad.Speed[_YAW] = Origion_NamelessQuad.Speed[_YAW] + vel_correction[_YAW];
}
#endif
